#include "shell.h"
#include "main.h"
#include "stdio.h"
#include "mc_config.h"
#include "mc_type.h"
#include "mc_tasks.h"
#include "mc_interface.h"
#include "motor_test.h"


extern FOCVars_t FOCVars[NBR_OF_MOTORS];
extern MCI_Handle_t * oMCInterface[NBR_OF_MOTORS];
extern motorTestcmd_t motorTestCMD;
char in_ccram_buffer[1024] __attribute__((section("ccmram")));

typedef enum
{
    PMSM_CMD_START_STOP = 0U,
    PID_PARAM = 1U,
    CURRENT_PARAM = 2U,
    PMSM_CMD_SPEED = 3U,
    PMSM_CMD_TORQUE,
    PMSM_CMD_SQURE_SPEED,
    PMSM_CMD_ACK,
}INPUT_CMD;

void motor_cmd(int cmd, int param)
{

    if (cmd >= 0 && (param >= -32767 && param < 32767))
    {
        static uint8_t cmdStatus = 0;
        switch (cmd)
        {
        case PMSM_CMD_START_STOP://start stop motor with default speed
            if(MC_GetSTMStateMotor1() == IDLE)
            {
                MC_StartMotor1();
            }
            else
            {
                MC_StopMotor1();
            }
            break;
        case PID_PARAM:
            break;
        case CURRENT_PARAM://printf current of motor
            for(uint8_t i = 0; i < param; i++)
            {
                printf("%d %d  %d %d  %d %d  %d\r\n",\
                FOCVars->Iab.a, FOCVars->Iab.b, FOCVars->Vqd.d, FOCVars->Vqd.q, \
                FOCVars->Iqdref.d, FOCVars->Iqdref.q, \
                PIDIqHandle_M1.wIntegralTerm);
            }
            break;
        case PMSM_CMD_SPEED:
            //real rpm = input param * 6, such as you input a -50 ,will get rpm 300 at a invert derection
            //max is 1800
            if(param > -2500 && param < 2500 )
            {
            	float rad_real = param / 6.0f;
                STC_SetControlMode(oMCInterface[M1]->pSTC, STC_SPEED_MODE);
                // STC_ExecRamp(oMCInterface[M1]->pSTC, )
                MCI_ExecSpeedRamp(oMCInterface[M1], (int16_t)rad_real, 500);
            }
            else
            {
            	printf("Invalid Input, Need -2500 ~ 2500 (rad/min)\r\n");
            }

            break;
        case PMSM_CMD_TORQUE:
            if(param > -3000 && param < 3000)//3A
            {
            	float input_current =  param * 3.28f;
                STC_SetControlMode(oMCInterface[M1]->pSTC, STC_TORQUE_MODE);
                MCI_ExecTorqueRamp(oMCInterface[M1],(int16_t)input_current, 1000);
            }
            else
            {
            	printf("Invalid Input, Need -3000 ~ 3000 (mA)\r\n");
            }
            break;
        case PMSM_CMD_SQURE_SPEED:
                if(cmdStatus == 0)
                {
                    STC_SetControlMode(oMCInterface[M1]->pSTC, STC_SPEED_MODE);
                    printf("start squre speed test!\r\n");
                    motorTestCMD.speedSqure = 1;
                    motorTestCMD.speedSine = 0;
                    motorTestCMD.tourqeSine = 0;
                    motorTestCMD.tourqeSqure = 0;
                    cmdStatus = 1;
                }
                else
                {
                    printf("stop squre speed test!\r\n");
                    motorTestCMD.speedSqure = 0;
                    motorTestCMD.speedSine = 0;
                    motorTestCMD.tourqeSine = 0;
                    motorTestCMD.tourqeSqure = 0;
                    cmdStatus = 0;
                    MC_StopMotor1();
                    memset(in_ccram_buffer, 5, 1024);
                    for(uint16_t i = 0; i < 1024; i++)
                    {
                        printf("%d ",in_ccram_buffer[i]);
                    }
                    printf("\r\n");
                }

                /**/
            break;
        case PMSM_CMD_ACK:
            STM_FaultAcknowledged(oMCInterface[M1]->pSTM);
            break;
        default:

            break;
        }
    }
    else
    {
        printf("input format error!\r\n");
    }
}

SHELL_EXPORT_CMD(motor_cmd, motor_cmd, cmd 0: FOC_PARAM 1:PID_PARAM 2:CURRENT_PARAM);